#!/usr/bin/env python3
import hardware
import moving
import vision
import KociembaTrans
import kociemba
import time
import cv2

arm_acting_time_90 = 0.2
arm_acting_time_180 = 0.23
hand_acting_time = 0.12
scan_time = 0.1

def scan():
    vision.update_img_flag(1)
    time.sleep(scan_time)
    hardware.loosen_hand0()
    time.sleep(hand_acting_time)

    hardware.arm1_rotate180_clockwise()
    time.sleep(arm_acting_time_180)
    vision.update_img_flag(2)
    time.sleep(scan_time)

    hardware.arm1_rotate90_anticlockwise()
    time.sleep(arm_acting_time_90)
    hardware.catch_hand0()
    time.sleep(hand_acting_time)
    hardware.loosen_hand1()
    time.sleep(hand_acting_time)
    hardware.arm1_rotate90_clockwise()
    time.sleep(arm_acting_time_90)
    hardware.catch_hand1()
    time.sleep(hand_acting_time)
    hardware.loosen_hand0()
    time.sleep(hand_acting_time)
    vision.update_img_flag(3)
    time.sleep(scan_time)

    hardware.arm1_rotate180_clockwise()
    time.sleep(arm_acting_time_180)
    vision.update_img_flag(4)
    time.sleep(scan_time)


    hardware.catch_hand0()
    time.sleep(hand_acting_time)
    hardware.loosen_hand1()
    time.sleep(hand_acting_time)
    hardware.arm0_rotate180_clockwise()
    time.sleep(arm_acting_time_180)
    vision.update_img_flag(5)
    time.sleep(scan_time)
    vision.update_img_flag(0)
    hardware.catch_hand1()
    time.sleep(hand_acting_time)
    while (vision.sovingdown_flag == 0):
        time.sleep(0.01)
    vision.sovingdown_flag = 0
    return vision.cubestate


def SolvingCube():
    hardware.catch_hand0()
    hardware.catch_hand1()
    time_scan_begin = time.time()
    CubeState = scan()
    print(time.time()-time_scan_begin)
    
    print(CubeState)
    # input()
    # CubeState = 'DRLUUBFBRBLURRLRUBLRDDFDLFUFUFFDBRDUBRUFLLFDDBFLUBLRBD'
    # CubeState = 'UUUUUUUUUBBBRRRFFFRRRFFFLLLDDDDDDDDDFFFLLLBBBLLLBBBRRR'
    # CubeState = ('U B B2 D\'')
    Solution = kociemba.solve(CubeState)
    hardware.faster_Arms()
    RealSolution,cost = KociembaTrans.TransToRealSolution(Solution)
    print(Solution)
    print(RealSolution)
    print(cost)
    moving.Solution_Optimize(RealSolution)
    # for Step in RealSolution:
    #     moving.Moving(Step)
    hardware.wait_cube_hand()
if __name__ == '__main__':
    CubeState = 'FRLBUBLLFDUBBRFBDBUFLFFRUUURLRLDRLFRRLFBLDUDFDUDUBDDRB'
    Solution = kociemba.solve(CubeState)
    hardware.faster_Arms()
    RealSolution,cost = KociembaTrans.TransToRealSolution(Solution)
    print(Solution)
    print(RealSolution+cost)